#include "fusion.h"

/*-------------------------Data pre_processing related-------------------------*/
/*
 * 	Function: KalmanFilter 
 * 	Parameter:
                data: the sensor data
 * 	Return: 
                the value of filter
 */
double KalmanFilter(double ResrcData)
{
    static double p_last = 0;
    static double x_last = 0;
    double p_now = P_Q;

    double R = M_R;
    double Q = P_Q;

    double x_mid = x_last;
    double x_now;

    double p_mid;
    double kg;

    //这里p_last 等于 kalmanFilter_A 的p直接取0
    x_mid = x_last;         //x_last=x(k-1|k-1),x_mid=x(k|k-1)
    p_mid = p_last + p_now; //p_mid=p(k|k-1),p_last=p(k-1|k-1),Q=噪声

    kg = p_mid / (p_mid + R);                 //kg为kalman filter，R 为噪声
    x_now = x_mid + kg * (ResrcData - x_mid); //估计出的最优值
    p_now = (1 - kg) * p_mid;                 //最优值对应的covariance
    p_last = p_now;                           //更新covariance 值
    x_last = x_now;                           //更新系统状态值

    return x_now;
}

/*
 * 	Function: data_pre_processing 
 * 	Parameter:
                data: the sensor data (arrray)
                len :the length of data arrray
 * 	Return: 
                the value of filter
 */
double data_pre_processing(double data[],int len)
{
    int i;
    double pre_processing_result;
    for (i = 0; i < len; i++)
    {
        pre_processing_result=KalmanFilter(data[i]);
    }
    return pre_processing_result;
}

/*-------------------------Fire monitor  related-------------------------*/
/*
 * 	Function: form membership 
 * 	Parameter:
                data:   sensor data
                x1:     the threshold value x1
                x2:     the threshold value x2
                x3:     the threshold value x3
                y1:     membership y1
                y2:     membership y1
 * 	Return: 
                membership
 */
float Membership(float data, float x1, float x2, float x3, float y1, float y2)
{
    float pro = 0.0;
    if (data < x1){
        pro = y1;
    }
    else if (x1 <= data < x2){
        pro = ((data - x1) * (y2 - y1) / (x2 - x1)) + y1;
    }
    else if (x2 <= data < x3){
        pro = ((data - x2) * (1 - y2) / (x3 - x2)) + y2;
    }
    else{
        pro = 1;
    }
    return pro;
}

/*
 * 	Function: data_fusion
 * 	Parameter:      
                Fire_data: buffer that stores fire_data
 * 	Return: 
                fusion result
 */
float data_fusion(float Fire_data[])
{
    float Tem_data_membership, Smog_data_membership, CO_data_membership;
    float data_fusion = 0.0;

    printf("tem:%f Smog:%f CO:%f \n",Fire_data[0],Fire_data[1],Fire_data[2]);

    Tem_data_membership = Membership(Fire_data[0], 30, 55, 80, 0.1, 0.5);       //温度的隶属度
    Smog_data_membership = Membership(Fire_data[1], 300, 1000, 4000, 0.1, 0.5); //烟雾的隶属度
    CO_data_membership = Membership(Fire_data[2], 30, 100, 200, 0.1, 0.5);      //CO的隶属度

    data_fusion = (Tem_data_membership + Smog_data_membership + CO_data_membership) / 3; //有火灾发生的概率
    printf("fusion result:%f\n",data_fusion);
    return data_fusion;
}

/*
 * 	Function: calculate the mean of float data
 * 	Parameter:
                data: float data
                length: the length of float data
 * 	Return:     
                the mean of data
 */
double mean(double data[], int length)
{
    int i;
    double sum = 0, aver;
    for (i = 0; i < length; i++)
        sum += data[i];
    aver = sum / length;
    return aver;
}

/*-------------------------air quality index evaluate fusion  related-------------------------*/
/*  
    Function:   Get the interval of air pollution quality index
    Parameter:  
                params_arr: buffer that stores 
                value:
                max: 
    Return: none
*/
float get_aqi_interval(int params_arr[], float value, int max)
{
    float IAQI_arr[] = {0, 50, 100, 150, 200, 300, 400, 500}; //空气质量分指数
    float ihigh, ilow, high, low, result;
    int i;
    for (i = 0; i < max; i++)
    {
        if (params_arr[i] < value && value < params_arr[i + 1])
        {
            ihigh = IAQI_arr[i + 1];
            ilow = IAQI_arr[i];
            high = params_arr[i + 1];
            low = params_arr[i];
            result = (ihigh - ilow) * (value - low) / (high - low) + ilow;
        }
    }
    return result;
}

/*  
    Function: convert a four byte unsigned char into ip string
    Parameter:  ip32: buffer that stores four byte unsigned char
                ip: ip string
    Return: none
*/
float *AQI_Weight(float AQI_array[])
{
    int Tox_coe[] = {1, 2, 5, 5, 4, 3};            /* Toxicity index of evaluation factors */
    float *W = (float *)malloc(sizeof(float) * 6); /* the weight of AQI impact factors */
    float sum = 0.0, sum_weight = 0.0;
    int i = 0, j = 0, k = 0;

    /*Calculate the weight of AQI impact factors */
    for (i = 0; i < 6; i++)
        sum += AQI_array[i];

    for (j = 0; j < 6; j++)
    {
        W[j] = AQI_array[j] / sum;
        sum_weight += W[j] / Tox_coe[j];
    }

    for (k = 0; k < 6; k++)
        W[k] = W[k] / (sum_weight * Tox_coe[k]);

    return W;
}

/*
 * 	Function:      calculate_AQI
 *  Description:   calculate AQI for each type of pollutant 
 * 	Parameter In：
 *                 type :  The pollutant_type (type：PM10:0x00,PM2.5:0x01, CO:0x02, NO2:0x03, O3:0x04, SO2:0x05 )
 *                 value : the pollutant_value
 * 	Parameter Out: AQI index
 */
float calculate_AQI(int type, float value)
{
    float AQI = 0.0;
    int PM10_arr[8] = {0, 50, 150, 250, 350, 420, 500, 600};
    int PM2_5_arr[8] = {0, 35, 75, 115, 150, 250, 350, 500};
    int CO_arr[8] = {0, 2, 4, 14, 24, 36, 48, 60};
    int NO2_arr[8] = {0, 50, 150, 475, 800, 1600, 2100, 2620};
    int O3_arr[6] = {0, 100, 160, 215, 265, 800};
    int SO2_arr[8] = {0, 40, 80, 180, 280, 565, 750, 940};
    switch (type)
    {
    case PM10:
        AQI = get_aqi_interval(PM10_arr, value, 8);
        break;
    case PM2_5:
        AQI = get_aqi_interval(PM2_5_arr, value, 8);
        break;
    case AIR_CO:
        AQI = get_aqi_interval(CO_arr, value, 8);
        break;
    case NO2:
        AQI = get_aqi_interval(NO2_arr, value, 8);
        break;
    case O3:
        AQI = get_aqi_interval(O3_arr, value, 6);
        break;
    default:
        AQI = get_aqi_interval(SO2_arr, value, 8);
    }
    return AQI;
}

/*  
    Function: fusion the aqi of the six sensor data
    Parameter:  data: buffer that stores six air quality sensor data
    Return: air quality index and air quality grade
*/
float AQI_fusion(float data[])
{
    int i = 0, j = 0, k = 0;
    /* Air quality assessment results */
    float AQI;
    float *AQI_GAS = (float *)malloc(sizeof(float) * air_quality_sensor_num);
    float *Weight = (float *)malloc(sizeof(float) * air_quality_sensor_num);
    printf("pm10:%f pm2.5:%f CO:%f NO2:%f O3:%f SO2:%f \n",data[0],data[1],data[2],data[3],data[4],data[5]);
    /* Calculate AQI for each pollutant */
    for (i = 0; i < air_quality_sensor_num; i++) 
    {
        AQI_GAS[i] = calculate_AQI(i, data[i]);
    }

    /* Calculate the weight  */
    Weight = AQI_Weight(AQI_GAS); 

    /* Weighted calculation of AQI index  */
    for (j = 0; j < air_quality_sensor_num; j++) 
    {
        AQI+= Weight[j] * AQI_GAS[j];
    }

    /* Judgment of air quality evaluation grade */
    // if ((AQI[0] >= 0) && (AQI[0] <= 50))

    //     AQI[1] = 1;

    // else if ((AQI[0] > 50) && (AQI[0] <= 100))

    //     AQI[1] = 2;

    // else if ((AQI[0] > 100) && (AQI[0] <= 150))

    //     AQI[1] = 3;

    // else if ((AQI[0] > 150) && (AQI[0] < 200))

    //     AQI[1] = 4;

    // else if ((AQI[0] > 200) && (AQI[0] <= 300))

    //     AQI[1] = 5;

    // else if ((AQI[0] > 300) && (AQI[0] <= 400))

    //     AQI[1] = 6;

    // else
    //     AQI[1] = 7;

    return AQI;
}
/*-------------------------water fusion related-------------------------*/
/*  
    Function: convert a four byte unsigned char into ip string
    Parameter:  
                Water_array: the data of water quality sensor
    Return:     the weight of water quality sensor
*/
float *calculate_water_Weight(float Water_array[])
{
    int Tox_coe[] = {1, 2, 4, 3}; //评价因子的毒性指数
    float *W = (float *)malloc(sizeof(float) * 4);
    float sum = 0.0, sum_weight = 0.0;
    int i = 0, j = 0, k = 0;

    //计算Water影响因子引起的权重
    for (i = 0; i < 4; i++)
    {
        sum += Water_array[i];
    }

    for (j = 0; j < 4; j++)
    {
        W[j] = Water_array[j] / sum;
        sum_weight += W[j] / Tox_coe[j];
    }

    for (k = 0; k < 4; k++)
    {
        W[k] = W[k] / (sum_weight * Tox_coe[k]);
    }

    return W;
}

/*  
    Function:   Get the interval of eater pollution quality index
    Parameter:  
                params_arr: buffer that stores 
                value:
                max: 
    Return: none
*/
float get_water_interval(float params_arr[], float value, int max)
{
    float IWater_arr[] = {0, 20, 40, 60, 80, 100}; //水质质量分指数
    float ihigh, ilow, high, low, result;
    int i;
        if(value > 2)
        {
            for ( i = 0; i < max; i++)
            {
                if (params_arr[i] > value && value > params_arr[i + 1])
                {
                    ihigh = IWater_arr[i + 1];
                    ilow = IWater_arr[i];
                    high = params_arr[i + 1];
                    low = params_arr[i];
                    result = (ihigh - ilow) * (value - low) / (high - low) + ilow;
                }
            }
        }
        else for (i = 0; i < max; i++)
        {
            if (params_arr[i] < value && value < params_arr[i + 1])
            {
                ihigh = IWater_arr[i + 1];
                ilow = IWater_arr[i];
                high = params_arr[i + 1];
                low = params_arr[i];
                result = (ihigh - ilow) * (value - low) / (high - low) + ilow;
            }
        }
        return result;
}

/*  
    Function:   Get the interval of water pollution quality index
    Parameter:  
                type: 
                value: 
    Return: none
*/
float cal_water_index(int type, float value)
{
    float Water = 0.0;
    float DO_arr[] = {10, 7.5, 6, 5, 3, 2};
    float NH3N_arr[] = {0, 0.15, 0.5, 1.0, 1.5, 2.0};
    float TP_arr[] = {0, 0.01, 0.025, 0.05, 0.1, 0.2};
    float TN_arr[] = {0, 0.2, 0.5, 1.0, 1.5, 2.0};
    switch (type)
    {
    case 0:
        Water = get_water_interval(DO_arr, value, 8);
        break;
    case 1:
        Water = get_water_interval(NH3N_arr, value, 8);
        break;
    case 2:
        Water = get_water_interval(TP_arr, value, 6);
        break;
    default:
        Water = get_water_interval(TN_arr, value, 8);
    }
    return Water;
}
/*  
    Function: fusion the water of the four sensor data
    Parameter:  
            data: buffer that stores four water quality sensor data
    Return: 
            water quality index and water quality grade
*/
float *water_fusion(float data[])
{
    int i = 0, j = 0, k = 0;
    float *Water = (float *)malloc(sizeof(float) * 2); //水质质量评估结果，包含污染指数和评价等级
    float *Water_GAS = (float *)malloc(sizeof(float) * 4);
    float *Weight = (float *)malloc(sizeof(float) * 4);

    float t = 0.0;

    for (i = 0; i < 4; i++) //计算每个污染物对应的Water指数
    {
        Water_GAS[i] = cal_water_index(i, data[i]);
    }
    Weight = calculate_water_Weight(Water_GAS); //计算权重

    for (j = 0; j < 4; j++) //加权值
    {
        Water[0] += Weight[j] * Water_GAS[j];
    }

    t = Water[0];
    if (0.0 < t && t <= 20.0)
    {
        Water[1] = 1;
    }
    else if (20.0 < t && t <= 40.0)
    {
        Water[1] = 2;
    }
    else if (40.0 < t && t <= 60.0)
    {
        Water[1] = 3;
    }
    else if (60.0 < t && t <= 80.0)
    {
        Water[1] = 4;
    }
    else if (80.0 < t && t <= 100)
    {
        Water[1] = 5;
    }
    else
    {
        Water[1] = 0;
    }
    return Water;
}

/*-------------------------Control related-------------------------*/
/*
 * 	Function: cooperative_control
 * 	Parameter:
                fusion_result: fusion data
 * 	Return: 
                smaple frequency of sensor
 */
int cooperative_control(float fusion_result)
{
    int fre;
    fre=3000-fusion_result*3000;
    return fre;
}

